//
// Created by ZhaoXiaoFei on 2022/10/27.
// Last changed by: ZhaoXiaoFei.
// Last update: 2022/10/27.
//

#ifndef MSCKF_MONO_MSCKF_H
#define MSCKF_MONO_MSCKF_H

#include <vector>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <opencv2/core/mat.hpp>
#include <iostream>
#include <boost/math/distributions/chi_squared.hpp>
#include "msckf_mono/data_types.h"

namespace msckf_mono{

    class MSCKF{
    public:
        MSCKF() = default;
        ~MSCKF() = default;

        void Init(const Camera& camera,
                  const MSCKFParams& msckfParams,
                  const NoiseParams& noiseParams,
                  const ImuState& imu_state){
            camera_ = camera;
            msckf_params_ = msckfParams;
            noise_params_ = noiseParams;

            imu_state_ = imu_state;
            imu_state_.p_G_I_null = imu_state_.p_G_I;
            imu_state_.v_G_I_null = imu_state_.v_G_I;
            imu_state_.q_I_G_null = imu_state_.q_I_G;
//            std::cout << "p_G_I: " << imu_state_.p_G_I.transpose() << " q_I_G: " << imu_state_.q_I_G << " v_G_I: " << imu_state_.v_G_I.transpose() << std::endl;

            imu_covar_ = noiseParams.init_imu_covar;

            num_feature_tracks_residualized_ = 0;

            chi_squared_test_table.resize(99);
            for (int i = 1; i < 100; ++i) {
                boost::math::chi_squared chi_squared_dist(i);
                chi_squared_test_table[i - 1] = boost::math::quantile(chi_squared_dist, 0.05);
            }

        }

        void Propagate(const ImuData& pre_imu_data, const ImuData& cur_imu_data){

            ImuState imu_state_prop = PropagateMidPoint(imu_state_, pre_imu_data, cur_imu_data);

            double dT = cur_imu_data.time - pre_imu_data.time;

            Eigen::Vector3f un_gyr0, un_acc0;
            un_gyr0 = pre_imu_data.gyr - imu_state_.b_g;
            un_acc0 = pre_imu_data.acc - imu_state_.b_a;
            Eigen::Matrix3f q_IG =  imu_state_.q_I_G.toRotationMatrix();

            // F
            F_.setZero();
            F_.template block<3, 3>(0, 0) = -vectorToSkewSymmetric(un_gyr0);
            F_.template block<3, 3>(0, 3) = -Eigen::Matrix3f::Identity();
            F_.template block<3, 3>(6, 0) = -q_IG.transpose() * vectorToSkewSymmetric(un_acc0);
            F_.template block<3, 3>(6, 9) = -q_IG.transpose();
            F_.template block<3, 3>(12, 6) = Eigen::Matrix3f::Identity();

            // G
            G_.setZero();
            G_.template block<3, 3>(0, 0) = -Eigen::Matrix3f::Identity();
            G_.template block<3, 3>(3, 3) = Eigen::Matrix3f::Identity();
            G_.template block<3, 3>(6, 6) = -q_IG.transpose();
            G_.template block<3, 3>(9, 9) = Eigen::Matrix3f::Identity();

            F_ *= dT;
            Phi_ = F_.exp();

            //OC_KF： Modify the state transition matrix
            Phi_.template block<3, 3>(0, 0) = imu_state_prop.q_I_G.toRotationMatrix()
                                              * imu_state_.q_I_G_null.toRotationMatrix().transpose();

            Eigen::Vector3f u = imu_state_.q_I_G_null.toRotationMatrix() * imu_state_.g;
            Eigen::Matrix<float,1, 3> s = (u.transpose() * u).inverse() * u.transpose();

            Eigen::Matrix3f A1 = Phi_.template block<3, 3>(6, 0);
            Eigen::Vector3f temp = imu_state_.v_G_I_null - imu_state_prop.v_G_I;
            Eigen::Vector3f w1 = vectorToSkewSymmetric(temp) * imu_state_.g;
            Phi_.template block<3, 3>(6, 0) = A1 - (A1 * u - w1) * s;

            Eigen::Matrix3f A2 = Phi_.template block<3, 3>(12, 0);
            temp = dT * imu_state_.v_G_I_null + imu_state_.p_G_I_null - imu_state_prop.p_G_I;
            Eigen::Vector3f w2 = vectorToSkewSymmetric(temp) * imu_state_.g;
            Phi_.template block<3, 3>(12, 0) = A2 - (A2 * u - w2) * s;

            Eigen::Matrix<float, 15, 15> imu_cov_prop = Phi_ * (imu_covar_ + G_ * noise_params_.Q_imu * G_.transpose() * dT) * Phi_.transpose();

            // update
            imu_state_ = imu_state_prop;
            imu_state_.q_I_G_null = imu_state_.q_I_G;
            imu_state_.v_G_I_null = imu_state_.v_G_I;
            imu_state_.p_G_I_null = imu_state_.p_G_I;

            imu_covar_ = (imu_cov_prop + imu_cov_prop.transpose()) / 2.0;
            imu_cam_covar_ = Phi_ * imu_cam_covar_;
//            std::cout << "p_G_I: " << imu_state_.p_G_I.transpose() << " q_I_G: " << imu_state_.q_I_G << " v_G_I: " << imu_state_.v_G_I.transpose() << std::endl;

        }

        void AugmentState(const int state_id, const double& time){
            Eigen::Quaternionf q_C_G = camera_.q_C_I * imu_state_.q_I_G;
            q_C_G.normalize();
            Eigen::Vector3f p_G_C = imu_state_.p_G_I + imu_state_.q_I_G.toRotationMatrix().transpose() * camera_.p_I_C;

            CameraState cur_cam_state;
            cur_cam_state.time = time;
            cur_cam_state.q_C_G = q_C_G;
            cur_cam_state.p_G_C = p_G_C;
            cur_cam_state.state_id = state_id;

            if(!camera_states_.empty()){
                P_.resize(imu_covar_.rows() + cam_covar_.rows(), imu_covar_.cols() + cam_covar_.cols());
                P_.block<15, 15>(0, 0) = imu_covar_;
                P_.block(0, 15, 15, cam_covar_.cols()) = imu_cam_covar_;
                P_.block(15, 0, cam_covar_.rows(), 15) = imu_cam_covar_.transpose();
                P_.block(15, 15, cam_covar_.rows(), cam_covar_.cols()) = cam_covar_;
            }
            else{
                P_.resize(imu_covar_.rows(), imu_covar_.cols());
                P_ = imu_covar_;
            }

            if (P_.determinant() < -0.000001) {
                std::cout << "Covariance determinant is negative! P_: " << P_.determinant() << std::endl;
            }

            const int N = camera_states_.size();
            Eigen::MatrixXf J(15 + 6 * N + 6, 15 + 6 * N);
            J.setZero();
            J.template block(0, 0, 15 + 6 * N, 15 + 6 * N).setIdentity();

            J.template block(15 + 6 * N, 0, 3, 3) = camera_.q_C_I.toRotationMatrix();
            J.template block(15 + 6 * N + 3, 0, 3, 3) = -imu_state_.q_I_G.toRotationMatrix().inverse() * vectorToSkewSymmetric(camera_.p_I_C);
            J.template block(15 + 6 * N + 3, 12, 3, 3) = Eigen::Matrix3f::Identity();

            Eigen::MatrixXf P_aug = J * P_ * J.transpose();

            Eigen::MatrixXf P_aug_sync = (P_aug + P_aug.transpose()) / 2.0;

            P_aug = P_aug_sync;

            camera_states_.push_back(cur_cam_state);

            // update
            imu_covar_ = P_aug.block<15, 15>(0, 0);

            cam_covar_.resize(P_aug.rows() - 15,  P_aug.cols() - 15);
            cam_covar_ = P_aug.block(15, 15, P_aug.rows() - 15, P_aug.cols() - 15);

            imu_cam_covar_.resize(15, P_aug.cols() - 15);
            imu_cam_covar_ = P_aug.block(0, 15, 15, P_aug.cols() - 15);

        }

        void Update(const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>>& OpticalFlow_features,
                    const std::vector<size_t>& OpticalFlow_feature_ids){
            feature_track_to_residualizes_.clear();
            std::vector<size_t> need_remove_feature_ids;

            int id_iter = 0;
            for (auto tracked_feature_id: tracked_feature_ids_) {

                auto iter = find(OpticalFlow_feature_ids.begin(), OpticalFlow_feature_ids.end(), tracked_feature_id);
                bool is_valid = (iter != OpticalFlow_feature_ids.end());

                auto track_feature = tracked_features_.begin() + id_iter;

                if (is_valid) {
                    size_t index_dist = distance(OpticalFlow_feature_ids.begin(), iter);
                    track_feature->observations.push_back(OpticalFlow_features[index_dist]);

                    auto last_cam_state = camera_states_.end() - 1;
                    last_cam_state->observed_feature_ids.push_back(tracked_feature_id);

                    track_feature->cam_state_ids.push_back(last_cam_state->state_id);
                }

                if (!is_valid || (track_feature->observations.size() >=
                                  msckf_params_.max_track_length)) {
                    FeatureTrackToResidualize feature_track_to_res;

                    RemoveFeaturesOfCameraVector(tracked_feature_id, feature_track_to_res.cam_states,
                                                 feature_track_to_res.cam_state_container_indexes);

                    if(feature_track_to_res.cam_states.size() >= msckf_params_.min_track_length) {
                        feature_track_to_res.feature_id = track_feature->feature_id;
                        feature_track_to_res.observations = track_feature->observations;
                        feature_track_to_res.is_init = track_feature->is_init;
                        if (track_feature->is_init) feature_track_to_res.point = track_feature->point;

                        feature_track_to_residualizes_.push_back(feature_track_to_res);
                    }

                    need_remove_feature_ids.push_back(tracked_feature_id);
                }

                id_iter++;
            }

            for(auto need_remove_id : need_remove_feature_ids){
                auto track_iter = tracked_features_.begin();
                while (track_iter != tracked_features_.end()) {
                    if (track_iter->feature_id == need_remove_id) {
                        track_iter = tracked_features_.erase(track_iter);
                        break;
                    }
                    else
                        track_iter++;
                }

                auto corresponding_id = std::find(tracked_feature_ids_.begin(),
                                                  tracked_feature_ids_.end(), need_remove_id);

                if (corresponding_id != tracked_feature_ids_.end()) {
                    tracked_feature_ids_.erase(corresponding_id);
                }
            }

        }

        void AddFeatures(const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> &new_features,
                         const std::vector<size_t> &new_feature_ids){
            assert(new_features.size() == new_feature_ids.size());

            for(int i = 0; i < new_features.size(); i++){
                size_t new_id = new_feature_ids[i];
                if(std::find(tracked_feature_ids_.begin(), tracked_feature_ids_.end(), new_id)
                   == tracked_feature_ids_.end()){
                    FeatureTrack track;
                    track.feature_id = new_id;
                    track.observations.push_back(new_features[i]);

                    auto last_camera_state = camera_states_.end() - 1;
                    last_camera_state->observed_feature_ids.push_back(new_id);

                    track.cam_state_ids.push_back(last_camera_state->state_id);
                    tracked_features_.push_back(track);
                    tracked_feature_ids_.push_back(new_id);
                }
                else {
                    std::cout << "Error, added new feature that was already being tracked" << std::endl;
                    return;
                }
            }
        }

        void Marginlize(){
            if(feature_track_to_residualizes_.empty()){
                return;
            }

            int num_passed = 0, num_rejected = 0, total_n_obs = 0;
            std::vector<bool> valid_tracks;
            std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> points_vec;

            for(auto track = feature_track_to_residualizes_.begin(); track != feature_track_to_residualizes_.end(); track++){
                if(num_feature_tracks_residualized_ > 3 && !checkMotion(track->observations.front(), track->cam_states)){
                    num_rejected += 1;
                    valid_tracks.push_back(false);
                    continue;
                }

                Eigen::Vector3f point;
                point.setZero();
                bool is_valid = GetPositionByLM(track->cam_states, track->observations, point);

                if(is_valid){
                    track->is_init = true;
                    track->point = point;
                }

                points_vec.push_back(point);
                int n_obs = track->observations.size();

                if(!is_valid){
                    num_rejected += 1;
                    valid_tracks.push_back(false);
                }
                else{
                    num_passed += 1;
                    valid_tracks.push_back(true);
                    total_n_obs += n_obs;
                    num_feature_tracks_residualized_ += 1;
                }

            }

            if(num_passed == 0){
                return;
            }

            Eigen::MatrixXf H_o = Eigen::MatrixXf::Zero(2 * total_n_obs - 3 * num_passed,
                                                15 + 6 * camera_states_.size());
            Eigen::MatrixXf R_o = Eigen::MatrixXf::Zero(2 * total_n_obs - 3 * num_passed,
                                                2 * total_n_obs - 3 * num_passed);
            Eigen::VectorXf r_o(2 * total_n_obs - 3 * num_passed);

            Eigen::Vector2f rep;
            rep << noise_params_.u_var, noise_params_.v_var;

            int counter = 0;
            for(int iter = 0; iter < feature_track_to_residualizes_.size(); iter++){
                if(!valid_tracks[iter]) continue;

                FeatureTrackToResidualize track = feature_track_to_residualizes_[iter];

                Eigen::Vector3f point = points_vec[iter];
                // calc residual
                Eigen::VectorXf r_j(2 * track.cam_states.size());
                int temp_i = 0;
                for(auto c_state : track.cam_states){
                    Eigen::Vector3f  p_f_c = c_state.q_C_G.toRotationMatrix() * (point - c_state.p_G_C);
                    Eigen::Vector2f z_hat = p_f_c.head<2>() / p_f_c(2);
                    r_j.block<2, 1>(2 * temp_i, 0) = track.observations[temp_i] - z_hat;
                    temp_i++;
                }
                int n_obs = track.observations.size();
                Eigen::MatrixXf R_j = (rep.replicate(n_obs, 1)).asDiagonal();

                //calc H_o_j A_j
                Eigen::MatrixXf H_o_j, A_j;
                Eigen::MatrixXf H_f_j = Eigen::MatrixXf::Zero(2 * track.cam_state_container_indexes.size(), 3);
                Eigen::MatrixXf H_x_j = Eigen::MatrixXf::Zero(2 * track.cam_state_container_indexes.size(), 15 + 6 * camera_states_.size());
                for(int c_i = 0; c_i < track.cam_state_container_indexes.size(); c_i++){
                    size_t index = track.cam_state_container_indexes[c_i];
                    Eigen::Vector3f p_f_C = camera_states_[index].q_C_G.toRotationMatrix() * (point - camera_states_[index].p_G_C);
                    Eigen::Matrix<float, 2, 3> J_i;
                    J_i << 1, 0, -p_f_C(0) / p_f_C(2), 0, 1, -p_f_C(1) / p_f_C(2);
                    J_i *= 1 / p_f_C(2);

                    Eigen::Matrix<float, 2, 6> A;
                    A << J_i * vectorToSkewSymmetric(p_f_C)
                            , -J_i * camera_states_[index].q_C_G.toRotationMatrix();

                    //OC_KF ： Modify observation matrix
                    Eigen::Matrix<float, 6, 1> u; u.setZero();
                    u.head(3) = camera_states_[index].q_C_G.toRotationMatrix() * imu_state_.g;
                    Eigen::Vector3f temp = point - camera_states_[index].p_G_C;
                    u.tail(3) = vectorToSkewSymmetric(temp) * imu_state_.g;

                    Eigen::Matrix<float, 2, 6> H_x = A - A * u * (u.transpose() * u).inverse() * u.transpose();
                    Eigen::Matrix<float, 2, 3> H_f = -H_x.block<2, 3>(0, 3);

                    H_f_j.block<2, 3>(2 * c_i, 0) = H_f;
                    H_x_j.block<2, 6>(2 * c_i, 15 + 6 * (index)) = H_x;
                }

                int jacobian_row_size = 2 * track.cam_state_container_indexes.size();
                Eigen::JacobiSVD<Eigen::MatrixXf> svd(H_f_j, Eigen::ComputeFullU | Eigen::ComputeFullV);
                A_j = svd.matrixU().rightCols(jacobian_row_size - 3);
                H_o_j = A_j.transpose() * H_x_j;
                Eigen::VectorXf r_o_j = A_j.transpose() * r_j;
                Eigen::MatrixXf R_o_j = A_j.transpose() * R_j * A_j;

                if(gatingTest(H_o_j, r_o_j, track.cam_states.size() - 1)) {
                    r_o.segment(counter, r_o_j.size()) = r_o_j;
                    H_o.block(counter, 0, H_o_j.rows(), H_o_j.cols()) = H_o_j;
                    R_o.block(counter, counter, R_o_j.rows(), R_o_j.cols()) = R_o_j;
                    counter += H_o_j.rows();
                }
            }

            H_o.conservativeResize(counter, H_o.cols());
            r_o.conservativeResize(counter);
            R_o.conservativeResize(counter, counter);

            MeasurementUpdate(H_o, r_o, R_o);
        }

        void pruneEmptyStates() {
            int max_states = msckf_params_.max_cam_states;
            if (camera_states_.size() < max_states) return;
            std::vector<size_t> deleteIdx;
            deleteIdx.clear();

            size_t num_states = camera_states_.size();

            // Find all cam_states_ with no tracked landmarks and prune them
            auto camState_it = camera_states_.begin();
            size_t num_deleted = 0;
            int camstate_pos = 0;
            int num_cam_states = camera_states_.size();

            int last_to_remove = num_cam_states - max_states - 1;

            if (camera_states_.front().observed_feature_ids.size()) {
                return;
            }

            for (int i = 1; i < num_cam_states - max_states; i++) {
                if (camera_states_[i].observed_feature_ids.size()) {
                    last_to_remove = i - 1;
                    break;
                }
            }

            for (int i = 0; i <= last_to_remove; ++i) {
                deleteIdx.push_back(camstate_pos + num_deleted);
                pruned_states_.push_back(*camState_it);
                camState_it = camera_states_.erase(camState_it);
                num_deleted++;
            }

            if (deleteIdx.size() != 0) {
                int n_remove = 0;
                int n_keep = 0;
                std::vector<bool> to_keep(num_states, false);
                for (size_t IDx = 0; IDx < num_states; IDx++) {
                    if (find(deleteIdx.begin(), deleteIdx.end(), IDx) != deleteIdx.end())
                        n_remove++;
                    else {
                        to_keep[IDx] = true;
                        n_keep++;
                    }
                }

                int remove_counter = 0;
                int keep_counter = 0;
                Eigen::VectorXi keepCovarIdx(6 * n_keep);
                Eigen::VectorXi removeCovarIdx(6 * n_remove);
                for (size_t IDx = 0; IDx < num_states; IDx++) {
                    if (!to_keep[IDx]) {
                        removeCovarIdx.segment<6>(6 * remove_counter) =
                                Eigen::VectorXi::LinSpaced(6, 6 * IDx, 6 * (IDx + 1) - 1);
                        remove_counter++;
                    } else {
                        keepCovarIdx.segment<6>(6 * keep_counter) =
                                Eigen::VectorXi::LinSpaced(6, 6 * IDx, 6 * (IDx + 1) - 1);
                        keep_counter++;
                    }
                }

                Eigen::MatrixXf prunedCamCovar;
                square_slice(cam_covar_, keepCovarIdx, prunedCamCovar);
                cam_covar_.resize(prunedCamCovar.rows(), prunedCamCovar.cols());
                cam_covar_ = prunedCamCovar;

                Eigen::Matrix<float, 15, Eigen::Dynamic> prunedImuCamCovar;
                column_slice(imu_cam_covar_, keepCovarIdx, prunedImuCamCovar);
                imu_cam_covar_.resize(prunedImuCamCovar.rows(), prunedImuCamCovar.cols());
                imu_cam_covar_ = prunedImuCamCovar;
            }

        }

        inline ImuState GetImuState() {
            return imu_state_;
        }

    private:

        ImuState PropagateMidPoint(const ImuState& imu_state, const ImuData& pre_imu_data, const ImuData& cur_imu_data){
            ImuState imuStateProp = imu_state;
            double dT = cur_imu_data.time - pre_imu_data.time;

            // Note: MSCKF quaternion form: -x,-y,-z,w
            //     Eigen quaternion is of form: w,x,y,z

            Eigen::Vector3f un_gyr0 = pre_imu_data.gyr - imu_state.b_g;
            Eigen::Vector3f un_gyr1 = cur_imu_data.gyr - imu_state.b_g;
            Eigen::Vector3f un_gyr = 0.5 * (un_gyr0 + un_gyr1);
            Eigen::Quaternionf dq;
            dq.w() = 1;
            dq.vec() = -0.5 * un_gyr * dT;
            imuStateProp.q_I_G = dq * imu_state.q_I_G;
            imuStateProp.q_I_G.normalize();

            Eigen::Vector3f un_acc0 = imu_state.q_I_G.toRotationMatrix().transpose() * (pre_imu_data.acc - imu_state_.b_a);
            Eigen::Vector3f un_acc1 = imuStateProp.q_I_G.toRotationMatrix().transpose() * (cur_imu_data.acc - imu_state_.b_a);

            Eigen::Vector3f un_acc = 0.5 * (un_acc0 + un_acc1) + imu_state_.g;
            imuStateProp.v_G_I  = imu_state.v_G_I + un_acc * dT;
            imuStateProp.p_G_I = imu_state.p_G_I + imu_state.v_G_I * dT + 0.5 * un_acc * dT * dT;

            return imuStateProp;
        }

        void RemoveFeaturesOfCameraVector(const size_t featureID, std::vector<CameraState>& feature_cam_states,
                                    std::vector<size_t>& cam_state_container_indexes){
            feature_cam_states.clear();
            cam_state_container_indexes.clear();

            for(int i = 0; i < camera_states_.size(); i++){
                auto iter = std::find(camera_states_[i].observed_feature_ids.begin(), camera_states_[i].observed_feature_ids.end(), featureID);
                if(iter != camera_states_[i].observed_feature_ids.end()){
                    camera_states_[i].observed_feature_ids.erase(iter);
                    feature_cam_states.push_back(camera_states_[i]);
                    cam_state_container_indexes.push_back(i);
                }
            }
        }

        void GetInitialValue(const Eigen::Matrix4f &T_c2_c0, const Eigen::Vector2f &obs0,
                             const Eigen::Vector2f &obs2, Eigen::Vector3f &point){
            // depth1 * x1 = depth2 * R * x2 + t;
            Eigen::Matrix3f R = T_c2_c0.template block<3, 3>(0, 0);
            Eigen::Vector3f t = T_c2_c0.template block<3, 1>(0, 3);

            Eigen::Vector3f m = R * Eigen::Vector3f(obs0(0), obs0(1), 1);
            Eigen::Vector2f A;
            A.setZero();
            A(0) = m(0) - m(2) * obs2(0);
            A(1) = m(1) - m(2) * obs2(1);

            Eigen::Vector2f b;
            b.setZero();
            b(0) = t(2) * obs2(0) - t(0);
            b(1) = t(2) * obs2(1) - t(1);

            float depth = (A.transpose() * A).inverse() * A.transpose() * b;
            point(0) = obs0(0) * depth;
            point(1) = obs0(1) * depth;
            point(2) = depth;
        }

        bool GetPositionByLM(const std::vector<CameraState>& camera_states,
                       const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>>& observations,
                       Eigen::Vector3f& point){

            std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> cam_poses;
            for(auto& cam : camera_states){
                Eigen::Matrix4f T_GC; T_GC.setIdentity();
                T_GC.template block<3, 3>(0, 0) = cam.q_C_G.toRotationMatrix().transpose();
                T_GC.template block<3, 1>(0, 3) = cam.p_G_C;
                cam_poses.push_back(T_GC);
            }
            Eigen::Matrix4f T_GC0 = cam_poses.front();
            for(auto& pose : cam_poses){
                pose = pose.inverse() * T_GC0;
            }
            Eigen::Vector3f initial_position(0.0, 0.0, 0.0);
            GetInitialValue(cam_poses[cam_poses.size() - 1], observations[0],
                                    observations[observations.size() - 1], initial_position);

            Eigen::Vector3f solution(initial_position(0) / initial_position(2),
                                     initial_position(1) / initial_position(2),
                                     1 / initial_position(2));

            int outer_loop_max_iterattion = 10;
            int inner_loop_max_iteration = 10;
            int inner_loop_cntr = 0;
            float total_cost = 0.0;
            float lambda = 1e-3;
            bool is_cost_reduced = false;
            float delta_x_norm = 0.0;
            float min_delta_x = 5e-7;

            for(int i = 0; i < cam_poses.size(); i++){

                Eigen::Matrix3f R = cam_poses[i].block<3, 3>(0, 0);
                Eigen::Vector3f t = cam_poses[i].block<3, 1>(0, 3);
                Eigen::Vector3f h = R * Eigen::Vector3f(solution(0),solution(1), 1.0) + solution(2) * t;
                Eigen::Vector2f z_hat(h(0) / h(2), h(1) / h(2));

                Eigen::Vector2f new_error = z_hat - observations[i];
                total_cost += new_error.squaredNorm();
            }

            for(int iter = 0; iter < outer_loop_max_iterattion && delta_x_norm > min_delta_x; iter++){

                Eigen::Matrix<float, 3, 3> A; A.setZero();
                Eigen::Vector3f            b; b.setZero();

                for(int i = 0; i < cam_poses.size(); i++){

                    Eigen::Matrix3f R = cam_poses[i].block<3, 3>(0, 0);
                    Eigen::Vector3f t = cam_poses[i].block<3, 1>(0, 3);
                    Eigen::Vector3f h = R * Eigen::Vector3f(solution(0),solution(1), 1.0) + solution(2) * t;
                    Eigen::Vector2f z_hat(h(0) / h(2), h(1) / h(2));

                    Eigen::Vector2f current_error = z_hat - observations[i];
                    //float current_cost = current_error.squaredNorm();
                    //total_cost += current_cost;

                    Eigen::Matrix<float, 2, 3> J, W1;
                    J.setZero(); W1.setZero();
                    W1 << 1 / h(2),   0,           -h(0) / (h(2) * h(2)),
                            0,         1 / h(2),   -h(1) / (h(2) * h(2));

                    Eigen::Matrix3f W2; W2.setZero();
                    W2.block<3, 2>(0, 0) = R.template block<3, 2>(0, 0);
                    W2.block<3, 1>(0, 2) = t;
                    J = W1 * W2;

                    float w = 1.0;
                    float e = current_error.norm();
                    float huber_epsilon = 0.01;
                    if(e <= huber_epsilon){
                        w = 1.0;
                    }else{
                        w = huber_epsilon / (2 * e);
                    }

                    if(w == 1){
                        A += J.transpose() * J;
                        b += J.transpose() * current_error;
                    }
                    else{
                        A += w * w * J.transpose() * J;
                        b += w * w * J.transpose() * current_error;
                    }

                }

                for(; inner_loop_cntr < inner_loop_max_iteration && !is_cost_reduced; inner_loop_cntr++){

                    Eigen::Vector3f delta_x = (A + lambda * Eigen::Matrix3f::Identity()).ldlt().solve(b);
                    Eigen::Vector3f new_solution = solution - delta_x;
                    delta_x_norm = delta_x.norm();

                    float new_cost = 0.0;
                    for(int i = 0; i < cam_poses.size(); i++){

                        Eigen::Matrix3f R = cam_poses[i].template block<3, 3>(0, 0);
                        Eigen::Vector3f t = cam_poses[i].template block<3, 1>(0, 3);
                        Eigen::Vector3f h = R * Eigen::Vector3f(new_solution(0),new_solution(1), 1.0) + new_solution(2) * t;
                        Eigen::Vector2f z_hat(h(0) / h(2), h(1) / h(2));

                        Eigen::Vector2f new_error = z_hat - observations[i];
                        new_cost += new_error.squaredNorm();

                    }

                    if (new_cost < total_cost) {
                        is_cost_reduced = true;
                        solution = new_solution;
                        total_cost = new_cost;
                        lambda = lambda / 10 > 1e-10 ? lambda / 10 : 1e-10;
                    } else {
                        is_cost_reduced = false;
                        lambda = lambda * 10 < 1e12 ? lambda * 10 : 1e12;
                    }

                }

                inner_loop_cntr = 0;
            }

            Eigen::Vector3f final_solution(solution(0) / solution(2), solution(1) / solution(2), 1.0 / solution(2));
            // check
            bool is_valid_solution = true;
            for (const auto &pose: cam_poses) {
                Eigen::Vector3f position = pose.block(0,0,3,3) * final_solution
                                            + pose.block(0,3,3,1);
                if (position(2) <= 0) {
                    is_valid_solution = false;
                    break;
                }
            }

            float normalized_cost = total_cost / (2 * cam_poses.size() * cam_poses.size());
            if(normalized_cost > msckf_params_.max_gn_cost_norm){
                is_valid_solution = false;
            }
            printf("Cost is: %f, normalized: %f\n", total_cost, normalized_cost);

            point = T_GC0.block<3, 3>(0,0) * final_solution + T_GC0.block<3, 1>(0, 3);

            return is_valid_solution;
        }

        void my_generateInitialGuess(const Eigen::Matrix4f &T_c2_c1, const Eigen::Vector2f &obs1,
                                     const Eigen::Vector2f &obs2, Eigen::Vector3f &point) const{
            // depth1 * x1 = depth2 * R * x2 + t;
            Eigen::Matrix3f R = T_c2_c1.template block<3, 3>(0, 0);
            Eigen::Vector3f t = T_c2_c1.template block<3, 1>(0, 3);

            Eigen::Vector3f m = R * Eigen::Vector3f(obs1(0), obs1(1), 1);
            Eigen::Vector2f A;
            A.setZero();
            A(0) = m(0) - m(2) * obs2(0);
            A(1) = m(1) - m(2) * obs2(1);

            Eigen::Vector2f b;
            b.setZero();
            b(0) = t(2) * obs2(0) - t(0);
            b(1) = t(2) * obs2(1) - t(1);

            float depth = (A.transpose() * A).inverse() * A.transpose() * b;
            point(0) = obs1(0) * depth;
            point(1) = obs1(1) * depth;
            point(2) = depth;
        }

        bool my_initialPosition(const std::vector<CameraState>& camera_states,
        const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>>& observations,
                Eigen::Vector3f& p_f_G){
            std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> cam_poses;

            for(auto& cam : camera_states){
                Eigen::Matrix4f T_GC; T_GC.setIdentity();
                T_GC.template block<3, 3>(0, 0) = cam.q_C_G.toRotationMatrix().transpose();
                T_GC.template block<3, 1>(0, 3) = cam.p_G_C;
                cam_poses.push_back(T_GC);
            }
            Eigen::Matrix4f T_GC0 = cam_poses.front();
            for(auto& pose : cam_poses){
                pose = pose.inverse() * T_GC0;
            }
            Eigen::Vector3f initial_position(0.0, 0.0, 0.0);
            my_generateInitialGuess(cam_poses[cam_poses.size() - 1], observations[0],
                                    observations[observations.size() - 1], initial_position);

            Eigen::Vector3f solution(initial_position(0) / initial_position(2),
                                     initial_position(1) / initial_position(2),
                                     1 / initial_position(2));
            int outer_loop_max_iterattion = 10;
            int inner_loop_max_iteration = 10;
            int inner_loop_cntr = 0;
            float total_cost = 0.0;
            float lambda = 1e-3;
            bool is_cost_reduced = false;
            float delta_x_norm = 0.0;
            float min_delta_x = 5e-7;

            for(int i = 0; i < cam_poses.size(); i++){

                Eigen::Matrix3f R = cam_poses[i].template block<3, 3>(0, 0);
                Eigen::Vector3f t = cam_poses[i].template block<3, 1>(0, 3);
                Eigen::Vector3f h = R * Eigen::Vector3f(solution(0),solution(1), 1.0) + solution(2) * t;
                Eigen::Vector2f z_hat(h(0) / h(2), h(1) / h(2));

                Eigen::Vector2f new_error = z_hat - observations[i];
                total_cost += new_error.squaredNorm();

            }

            for(int iter = 0; iter < outer_loop_max_iterattion && delta_x_norm > min_delta_x; iter++){

                Eigen::Matrix<float, 3, 3> A; A.setZero();
                Eigen::Vector3f            b; b.setZero();

                for(int i = 0; i < cam_poses.size(); i++){

                    Eigen::Matrix3f R = cam_poses[i].template block<3, 3>(0, 0);
                    Eigen::Vector3f t = cam_poses[i].template block<3, 1>(0, 3);
                    Eigen::Vector3f h = R * Eigen::Vector3f(solution(0),solution(1), 1.0) + solution(2) * t;
                    Eigen::Vector2f z_hat(h(0) / h(2), h(1) / h(2));

                    Eigen::Vector2f current_error = z_hat - observations[i];
                    //float current_cost = current_error.squaredNorm();
                    //total_cost += current_cost;

                    Eigen::Matrix<float, 2, 3> J, W1;
                    J.setZero(), W1.setZero();
                    W1 << 1 / h(2),   0,           -h(0) / (h(2) * h(2)),
                            0,         1 / h(2),   -h(1) / (h(2) * h(2));

                    Eigen::Matrix3f W2; W2.setZero();
                    W2.template block<3, 2>(0, 0) = R.template block<3, 2>(0, 0);
                    W2.template block<3, 1>(0, 2) = t;
                    J = W1 * W2;

                    float w = 1.0;
                    float e = current_error.norm();
                    float huber_epsilon = 0.01;
                    if(e <= huber_epsilon){
                        w = 1.0;
                    }else{
                        w = huber_epsilon / (2 * e);
                    }

                    if(w == 1){
                        A += J.transpose() * J;
                        b += J.transpose() * current_error;
                    }
                    else{
                        A += w * w * J.transpose() * J;
                        b += w * w * J.transpose() * current_error;
                    }

                }

                for(; inner_loop_cntr < inner_loop_max_iteration && !is_cost_reduced; inner_loop_cntr++){

                    Eigen::Vector3f delta_x = (A + lambda * Eigen::Matrix3f::Identity()).ldlt().solve(b);
                    Eigen::Vector3f new_solution = solution - delta_x;
                    delta_x_norm = delta_x.norm();

                    float new_cost = 0.0;
                    for(int i = 0; i < cam_poses.size(); i++){

                        Eigen::Matrix3f R = cam_poses[i].template block<3, 3>(0, 0);
                        Eigen::Vector3f t = cam_poses[i].template block<3, 1>(0, 3);
                        Eigen::Vector3f h = R * Eigen::Vector3f(new_solution(0),new_solution(1), 1.0) + new_solution(2) * t;
                        Eigen::Vector2f z_hat(h(0) / h(2), h(1) / h(2));

                        Eigen::Vector2f new_error = z_hat - observations[i];
                        new_cost += new_error.squaredNorm();

                    }

                    if (new_cost < total_cost) {
                        is_cost_reduced = true;
                        solution = new_solution;
                        total_cost = new_cost;
                        lambda = lambda / 10 > 1e-10 ? lambda / 10 : 1e-10;
                    } else {
                        is_cost_reduced = false;
                        lambda = lambda * 10 < 1e12 ? lambda * 10 : 1e12;
                    }

                }

                inner_loop_cntr = 0;
            }

            Eigen::Vector3f final_solution(solution(0) / solution(2), solution(1) / solution(2), 1.0 / solution(2));
            // check
            bool is_valid_solution = true;
            for (const auto &pose: cam_poses) {
                Eigen::Vector3f position =
                        pose.template block(0,0,3,3) * final_solution + pose.template block(0,3,3,1);
                if (position(2) <= 0) {
                    is_valid_solution = false;
                    break;
                }
            }

            float normalized_cost = total_cost / (2 * cam_poses.size() * cam_poses.size());
            if(normalized_cost > msckf_params_.max_gn_cost_norm){
                is_valid_solution = false;
            }
            printf("Cost is: %f, normalized: %f\n", total_cost, normalized_cost);

            p_f_G = T_GC0.template block<3, 3>(0,0) * final_solution + T_GC0.template block<3, 1>(0, 3);

            return is_valid_solution;
        }

        void MeasurementUpdate(const Eigen::MatrixXf& H_o,
                                  const Eigen::VectorXf& r_o,
                                  const Eigen::MatrixXf& R_o){
            if(r_o.size() == 0) return;

            Eigen::MatrixXf P = Eigen::MatrixXf::Zero(15 + cam_covar_.rows(), 15 + cam_covar_.cols());
            P.block(0, 0, 15, 15) = imu_covar_;
            if(cam_covar_.rows() != 0){
                P.block(0, 15, 15, imu_cam_covar_.cols()) = imu_cam_covar_;
                P.block(15, 0, imu_cam_covar_.cols(), 15) = imu_cam_covar_.transpose();
                P.block(15, 15, cam_covar_.rows(), cam_covar_.cols()) = cam_covar_;
            }

            Eigen::HouseholderQR<Eigen::MatrixXf> qr(H_o);
            Eigen::MatrixXf Q = qr.householderQ();
            Eigen::MatrixXf R = qr.matrixQR().triangularView<Eigen::Upper>();
            Eigen::VectorXf nonZeroRows = R.rowwise().any();
            int numNoneZeroRows = nonZeroRows.sum();

            Eigen::MatrixXf T_H = Eigen::MatrixXf::Zero(numNoneZeroRows, R.cols());
            Eigen::MatrixXf Q_1 = Eigen::MatrixXf::Zero(Q.rows(), numNoneZeroRows);

            int count = 0;
            for(int index = 0; index < R.rows(); index++){
                if(nonZeroRows[index] == 1.0){
                    T_H.row(count) = R.row(index);
                    Q_1.col(count) = Q.col(index);
                    count++;
                }
            }

            Eigen::MatrixXf r_n, R_n;
            r_n = Q_1.transpose() * r_o;
            R_n = Q_1.transpose() * R_o * Q_1;

            Eigen::MatrixXf K = P * T_H.transpose() * (T_H * P * T_H.transpose() + R_n).inverse();

            Eigen::VectorXf delta_x = K * r_n;
            Eigen::Quaternionf dq;
            Eigen::Vector3f delta_theta = 0.5 * delta_x.head(3);
            float check = delta_theta.squaredNorm();
            if(check > 1){
                dq.w() = 1; dq.x() = -delta_theta(0); dq.y() = -delta_theta(1); dq.z() = -delta_theta(2);
            }
            else{
                dq.w() = std::sqrt(1 - check); dq.x() = -delta_theta(0); dq.y() = -delta_theta(1); dq.z() = -delta_theta(2);
            }
            dq.normalize();

            imu_state_.q_I_G = dq * imu_state_.q_I_G;
            imu_state_.b_g += delta_x.segment<3>(3);
            imu_state_.b_a += delta_x.segment<3>(9);
            imu_state_.v_G_I += delta_x.segment<3>(6);
            imu_state_.p_G_I += delta_x.segment<3>(12);

            for (size_t c_i = 0; c_i < camera_states_.size(); c_i++) {

                Eigen::Quaternionf dq;
                Eigen::Vector3f delta_theta = 0.5 * delta_x.segment<3>(15 + 6 * c_i);
                float check = delta_theta.squaredNorm();
                if(check > 1){
                    dq.w() = 1; dq.x() = -delta_theta(0); dq.y() = -delta_theta(1); dq.z() = -delta_theta(2);
                }
                else{
                    dq.w() = std::sqrt(1 - check); dq.x() = -delta_theta(0); dq.y() = -delta_theta(1); dq.z() = -delta_theta(2);
                }
                dq.normalize();

                Eigen::Quaternionf q_CG_up = dq * camera_states_[c_i].q_C_G;
                camera_states_[c_i].q_C_G = q_CG_up.normalized();
                camera_states_[c_i].p_G_C += delta_x.template segment<3>(18 + 6 * c_i);
            }

            Eigen::MatrixXf tempMat = Eigen::MatrixXf::Identity(15 + 6 * camera_states_.size(), 15 + 6 * camera_states_.size()) - K * T_H;
            Eigen::MatrixXf P_corrected = tempMat * P * tempMat.transpose() + K * R_n * K.transpose();
            Eigen::MatrixXf P_corrected_transpose;
            P_corrected_transpose = P_corrected.transpose();
            P_corrected += P_corrected_transpose;
            P_corrected /= 2;

            if (P_corrected.rows() - 15 != cam_covar_.rows()) {
                std::cout << "[P:" << P_corrected.rows() << "," << P_corrected.cols() << "]";
                std::cout << "[cam_covar_:" << cam_covar_.rows() << "," << cam_covar_.cols() << "]";
                std::cout << std::endl;
            }

            imu_covar_ = P_corrected.block<15, 15>(0, 0);

            cam_covar_ = P_corrected.block(15,                      15,
                                           P_corrected.rows() - 15,P_corrected.cols() - 15);
            imu_cam_covar_ = P_corrected.block(0, 15, 15, P_corrected.cols() - 15);

        }

        bool checkMotion(const Eigen::Vector2f first_observation,
                         const std::vector<CameraState> &cam_states) {
            if (cam_states.size() < 2) {
                return false;
            }
            const CameraState &first_cam = cam_states.front();

            Eigen::Matrix4f first_cam_pose;
            first_cam_pose.block<3, 3>(0, 0) = first_cam.q_C_G.toRotationMatrix().transpose();
            first_cam_pose.block<3, 1>(0, 3) = first_cam.p_G_C;

            Eigen::Vector3f feature_direction;
            feature_direction << first_observation, 1.0;
            feature_direction = feature_direction / feature_direction.norm();
            feature_direction = first_cam_pose.block<3, 3>(0, 0) * feature_direction;

            float max_ortho_translation = 0;

            for (auto second_cam_iter = cam_states.begin() + 1;
                 second_cam_iter != cam_states.end(); second_cam_iter++) {
                Eigen::Matrix4f second_cam_pose;
                second_cam_pose.block<3, 3>(0, 0) = second_cam_iter->q_C_G.toRotationMatrix().transpose();
                second_cam_pose.block<3, 1>(0, 3) = second_cam_iter->p_G_C;

                Eigen::Vector3f translation = second_cam_pose.block<3, 1>(0, 3) - first_cam_pose.block<3, 1>(0, 3);
                float parallel_translation = translation.transpose() * feature_direction;
                Eigen::Vector3f orthogonal_translation = translation - parallel_translation * feature_direction;
                if (orthogonal_translation.norm() > max_ortho_translation) {
                    max_ortho_translation = orthogonal_translation.norm();
                }
            }

            if (max_ortho_translation > msckf_params_.translation_threshold)
                return true;
            else
                return false;
        }

        // Constraint on track to be marginalized based on Mahalanobis Gating
        // High Precision, Consistent EKF-based Visual-Inertial Odometry by Li et al.
        bool gatingTest(const Eigen::MatrixXf &H, const Eigen::VectorXf &r, const int &dof) {
            Eigen::MatrixXf P = Eigen::MatrixXf::Zero(15 + cam_covar_.rows(), 15 + cam_covar_.cols());
            P.template block<15, 15>(0, 0) = imu_covar_;
            if (cam_covar_.rows() != 0) {
                P.block(0, 15, 15, imu_cam_covar_.cols()) = imu_cam_covar_;
                P.block(15, 0, imu_cam_covar_.cols(), 15) = imu_cam_covar_.transpose();
                P.block(15, 15, cam_covar_.rows(), cam_covar_.cols()) = cam_covar_;
            }

            Eigen::MatrixXf P1 = H * P * H.transpose();
            Eigen::MatrixXf P2 =
                    noise_params_.u_var * Eigen::MatrixXf::Identity(H.rows(), H.rows());
            float gamma = r.transpose() * (P1 + P2).ldlt().solve(r);

            if (gamma < chi_squared_test_table[dof + 1]) {
                // cout << "passed" << endl;
                return true;
            } else {
                // cout << "failed" << endl;
                return false;
            }
        }


    private:
        Camera camera_;
        MSCKFParams msckf_params_;
        NoiseParams noise_params_;

        std::vector<size_t> tracked_feature_ids_;
        std::vector<FeatureTrack> tracked_features_;
        std::vector<CameraState> camera_states_;

        std::vector<FeatureTrackToResidualize> feature_track_to_residualizes_;
        int num_feature_tracks_residualized_;


        ImuState imu_state_;

        Eigen::Matrix<float, 15, 15> F_;
        Eigen::Matrix<float, 15, 15> Phi_;
        Eigen::Matrix<float, 15, 12> G_;

        Eigen::Matrix<float, 15, 15> imu_covar_;
        Eigen::Matrix<float, 15, Eigen::Dynamic> imu_cam_covar_;
        Eigen::MatrixXf cam_covar_;

        Eigen::MatrixXf P_;

        std::vector<float> chi_squared_test_table;
        std::vector<CameraState> pruned_states_;
        std::vector<size_t> tracks_to_remove_;
    };



}

#endif //MSCKF_MONO_MSCKF_H
